%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Sajad Saeedi
% University of New Brunswick
% Copy Right 2012
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% ref: 
% S. Saeedi, L. Paull, M. Trentini, M. Seto, and H. Li, 
% Map Merging Using Hough Peak Matching, 
% Proceedings of the IEEE/RSJ International Conference on 
% Intelligent Robots and Systems (IROS), 
% Vilamoura, Portugal, October 7-12, pp. 4683 - 4688.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

close all;
clear all;
clc;

%% read both maps

m1 = imread('fine_intel0.pgm');
m2 = imread('fine_intel2.pgm');

%% adjust orientation angle by radon transform
[im_occ1, im_occ2, im_occ2_rot, R1, T1, D1, R2, T2, D2, R2_rot, T2_rot, D2_rot, delta_ang] = adjust_radon(m1, m2);

figure; imshow(im_occ1, [-1 1]);
set(gcf,'name','map_1','numbertitle','off');
title('map_1', 'FontSize',12);
%xlim([180 640]); ylim([160 480]);

figure; imshow(im_occ2, [-1 1]);
set(gcf,'name','map_2','numbertitle','off');
title('map_2', 'FontSize',12);
% %xlim([180 640]); ylim([160 480]);
 
figure; imshow(im_occ2_rot, [-1 1]);
set(gcf,'name','rot-map_2','numbertitle','off');
title('aligned map_2', 'FontSize',12);
% %xlim([180 640]); ylim([160 480]);

%% Hough peaks
peaks1 = [];
peaks2 = [];
peaks2_rot = [];
peaks2_cc_rot = [];

% for Andrew's maps
max_no       = 80;
threshold    = 33;  % 34
d_gate       = 80;  % distance gate
v_gate       = 20;  % value gate
delta_theta  = 1;
n_hypothesis = 1;
theta_cells = 360;


H1 = -R1;

%% 计算并显示霍夫峰值
%peaks1 = GetRadonPeaks(H1, 100);
% peaks = houghpeaks(H,numpeaks) 定位由 hough 函数生成的 Hough 变换矩阵 H 中
% 的峰值。numpeaks 指定要识别的最大峰值个数。该函数返回 peaks，即一个保留峰值的行和列坐标的矩阵。
% peaks = houghpeaks(H,numpeaks,Name,Value) 使用名称-值对组参数控制运算的各个方面。
% peaks：找到的峰值的行和列坐标，但是我们这个是*3的一个向量
peaks1 = houghpeaks(H1, max_no, 'Threshold', threshold);

% peaks1 = houghpeaks(H1, 10);
% figure;imagesc(peaks1);
% set(gcf,'name','peaks_1','numbertitle','off');
% title('peaks_1', 'FontSize',12);
figure;
imshow(H1,[],'XData',T1,'YData',D1,'InitialMagnification','fit');
xlabel('\theta'), ylabel('\rho');
axis on, axis normal, hold on;
title('Hough image of map_1')
plot(T1(peaks1(:,2)),D1(peaks1(:,1)),'s','color','white');
set(gcf,'name','H-peaks1','numbertitle','off'); 
colormap(jet);

displayHoughspectrum(peaks1, T1);

H2  = -R2;
peaks2 = houghpeaks(H2, max_no, 'Threshold', threshold);

figure;
imshow(H2,[],'XData',T2,'YData',D2,'InitialMagnification','fit');
xlabel('\theta'), ylabel('\rho');
title('Hough image of map_2')
axis on, axis normal, hold on;
plot(T2(peaks2(:,2)),D2(peaks2(:,1)),'s','color','white');
set(gcf,'name','H-peaks2','numbertitle','off'); 
colormap(jet);

displayHoughspectrum(peaks2, T2);

H2_rot  = -R2_rot;
peaks2_rot = houghpeaks(H2_rot, max_no, 'Threshold', threshold);


figure;
imshow(H2_rot,[],'XData',T2_rot,'YData',D2_rot,'InitialMagnification','fit');
xlabel('\theta'), ylabel('\rho');
title('Hough image of rotated map_2')
axis on, axis normal, hold on;
plot(T2_rot(peaks2_rot(:,2)),D2_rot(peaks2_rot(:,1)),'s','color','white');
set(gcf,'name','H-peaks2_rot','numbertitle','off'); 
colormap(jet);

% 采用霍夫频谱计算旋转角度
[ang2_cc_rot, R2_cc_rot, T2_cc_rot, D2_cc_rot, rot_hat, im_occ2_cc_rot] = hs_compute_rot(H1, H2, im_occ2, theta_cells, delta_ang);
H2_cc_rot = -R2_cc_rot;

% 最终旋转过后的霍夫图与霍夫峰值
figure;
imshow(H2_cc_rot,[],'XData',T2_cc_rot,'YData',D2_cc_rot,'InitialMagnification','fit');
xlabel('\theta'), ylabel('\rho');
title('Hough image of rotated map_2')
axis on, axis normal, hold on;
peaks2_cc_rot = houghpeaks(H2_cc_rot, max_no, 'Threshold', threshold);
plot(T2_cc_rot(peaks2_cc_rot(:,2)),D2_cc_rot(peaks2_cc_rot(:,1)),'s','color','white');
set(gcf,'name','H-peaks2_cc_rot','numbertitle','off'); 
colormap(jet);


n1 = size(peaks1,1);
for i=1:n1
  id = [peaks1(i,1) peaks1(i,2)];
  peaks1(i,3) =  H1(id(1), id(2)); 
end

n2 = size(peaks2,1);
for i=1:n2
  id = [peaks2(i,1) peaks2(i,2)];
  peaks2(i,3) =  H2(id(1), id(2)); 
end

n2_rot = size(peaks2_rot,1);
for i=1:n2_rot
  id = [peaks2_rot(i,1) peaks2_rot(i,2)];
  peaks2_rot(i,3) =  H2_rot(id(1), id(2)); 
end

peaks1
peaks2
peaks2_rot


%% Calculate correspondence for Hough translation
crpd = -1*ones(n1, 6);
crpd(:,1:3) = peaks1;

for i = 1:n1
  ang = peaks1(i,2); % select an angle
  closest_id = -1;
  closest_dist = inf;
  
  for j = 1:n2_rot
    %if ang == peaks2(j,2)
    if abs(ang - peaks2_rot(j,2)) <= delta_theta
      delta_v = abs(peaks1(i,3)-peaks2_rot(j,3));
      if delta_v < v_gate
        delta_d = abs(peaks1(i,1)-peaks2_rot(j,1));
        if ((delta_d < d_gate) && (delta_d < closest_dist))
          closest_id = j;         
        end   
      end
    end  
  end
  
  if closest_id ~= -1
    crpd(i,4) = peaks2_rot(closest_id,1);
    crpd(i,5) = peaks2_rot(closest_id,2);
    crpd(i,6) = peaks2_rot(closest_id,3);
  end
end
crpd

%%% Solve from Hough
n = size(crpd,1);
A = [];
B = [];
prj_R1 = [];
prj_R1p = [];
ofset = sqrt(size(m1,1)^2+size(m1,2)^2)
crpd_no = 0;

for i =1:n
 if crpd(i,4) ~= -1          % if the angle is OK
   %if crpd(i,2) == crpd(i,5) % check if angles are the same
     theta1 = crpd(i,2)-91;         % this is the angle
     theta2 = crpd(i,5)-91;         % this is the angle
     theta  = (theta1 + theta2)/2;   
     A = [cosd(theta)   sind(theta); A];
     prj_R1  = [crpd(i,1) - ofset; prj_R1];
     prj_R1p = [crpd(i,4) - ofset; prj_R1p];
   %end
   crpd_no = crpd_no + 1;
 end
end
% theta = [0;   -45;   -90];
% R1    = [100;   0;   -100];
% R1p   = [171;   6;   -162];

A;
B = [prj_R1p - prj_R1];
T = inv(A'*A)*A'*B
crpd_no
%T = [0 39] for maz1 and maze2
im_occ2_rot_tr = map_transform(im_occ2_rot, 0, -fix(T)); %Tr should ve [-25 -1]
bothmaps = plotbothmaps(im_occ1, im_occ2_rot_tr, [-1 1]);

im_occ2_cc_rot_tr = map_transform(im_occ2_cc_rot, 0, -fix(T)); %Tr should ve [-25 -1]
bothmaps_cc = plotbothmaps(im_occ1, im_occ2_cc_rot_tr, [-1 1]);
